import numpy as np
def InputAugmentMatrix_Delta_U(A,B,Q,R,S,AD):
    n = len(A)
    p = B.shape[1]
    Ca = np.hstack((np.eye(n),-np.eye(n),np.zeros((n,p))))#横向堆叠
    
    Aa = np.vstack((np.hstack((A,np.zeros((n,n)),B)),np.hstack((np.zeros((n,n)),AD,np.zeros((n,p))))
                    ,np.hstack((np.zeros((p,n)),np.zeros((p,n)),np.eye(p)))))

    Ba = np.vstack((B,np.zeros((n,p)),np.eye(p)))
    Qa = Ca.T@Q@Ca
    Sa = Ca.T@S@Ca
    R = R
    return Aa,Ba,Qa,Sa,R